/*
 * Copyright  2017-2019 NXP
 * All rights reserved.
 *
 *
 * SPDX-License-Identifier: BSD-3-Clause
 */

#include "display_support.h"
#include "camera_support.h"
#include "fsl_pxp.h"

#include "pin_mux.h"
#include "board.h"
#include "fsl_debug_console.h"


#include "tos_k.h"
#include "fsl_device_registers.h"
#include "fsl_debug_console.h"
#include "pin_mux.h"
#include "clock_config.h"
#include "board.h"
#include "fsl_gpio.h"

void SysTick_Handler(void)
{
    if (tos_knl_is_running()) {
        tos_knl_irq_enter();
        tos_tick_handler();
        tos_knl_irq_leave();
    }
}

#define APPLICATION_TASK_STK_SIZE       40960
k_task_t application_task;
uint8_t application_task_stk[APPLICATION_TASK_STK_SIZE];

extern void application_entry(void *arg);

int main(void)
{
    BOARD_ConfigMPU();
    BOARD_InitBootPins();
    BOARD_InitDEBUG_UARTPins();
    BOARD_InitSDRAMPins();
    BOARD_EarlyPrepareCamera();
    BOARD_InitCSIPins();
    BOARD_InitLCDPins();
    BOARD_InitBootClocks();
    BOARD_InitDebugConsole();

	
    PRINTF("Welcome to TencentOS tiny(%s)\r\n", TOS_VERSION);
    tos_knl_init(); // TencentOS Tiny kernel initialize
    tos_task_create(&application_task, "application_task", application_entry, NULL, 4, application_task_stk, APPLICATION_TASK_STK_SIZE, 0);
    tos_knl_start();
    
    while (1)
		{
			PRINTF("while (1)\r\n");
		}
}

/*******************************************************************************
 * Definitions
 ******************************************************************************/
#define DEMO_PXP  PXP

#define APP_FRAME_BUFFER_COUNT 2
#define APP_CAMERA_BUFFER_COUNT 4

/* Camera's Pixel format RGB565, bytesPerPixel is 2. */
#define APP_CAMERA_BUFFER_BPP 2

/* LCD's Pixel format XRGB8888, bytesPerPixel is 4. */
#define APP_FRAME_BUFFER_BPP 4

#if (FRAME_BUFFER_ALIGN > DEMO_CAMERA_BUFFER_ALIGN)
#define DEMO_LCD_BUFFER_ALIGN FRAME_BUFFER_ALIGN
#else
#define DEMO_LCD_BUFFER_ALIGN DEMO_CAMERA_BUFFER_ALIGN
#endif

/*******************************************************************************
 * Prototypes
 ******************************************************************************/
static void APP_BufferSwitchOffCallback(void *param, void *switchOffBuffer);
static void APP_InitCamera(void);
static void APP_InitDisplay(void);
static void APP_CSI_RGB565(void);
static void APP_InitPxP(void);

/*******************************************************************************
 * Variables
 ******************************************************************************/
AT_NONCACHEABLE_SECTION_ALIGN(
    static uint32_t s_lcdBuffer[APP_FRAME_BUFFER_COUNT][DEMO_BUFFER_HEIGHT][DEMO_BUFFER_WIDTH],
    DEMO_LCD_BUFFER_ALIGN);

AT_NONCACHEABLE_SECTION_ALIGN(
    static uint16_t s_cameraBuffer[APP_CAMERA_BUFFER_COUNT][DEMO_CAMERA_HEIGHT][DEMO_CAMERA_WIDTH],
    DEMO_CAMERA_BUFFER_ALIGN);

uint32_t gray_data[DEMO_CAMERA_HEIGHT*DEMO_CAMERA_WIDTH>>2];
uint32_t gray_zoom_data[DEMO_CAMERA_HEIGHT*DEMO_CAMERA_WIDTH>>4];
/*
 * When new frame buffer sent to display, it might not be shown immediately.
 * Application could use callback to get new frame shown notification, at the
 * same time, when this flag is set, application could write to the older
 * frame buffer.
 */
static volatile bool s_newFrameShown = false;
static dc_fb_info_t fbInfo;
static volatile uint8_t s_lcdActiveFbIdx;

/*******************************************************************************
 * Code
 ******************************************************************************/

void convert_rgb565_to_gray(uint8_t *src,uint8_t *dst, int w,int h)
{
  int i, j;
  unsigned char *prgb;
  unsigned char gm_red, gm_green, gm_blue;
  unsigned short *pt;
  unsigned char *pr;

  pt = (unsigned short *)src;  // 
  pr = (unsigned char *)dst;

  for (j = 0; j < w; j++) 
  {
    for (i = 0; i < h; i++) 
    {
      gm_red = ((*(unsigned short *)pt) & 0xF800) >> 8;
      gm_green = ((*(unsigned short *)pt) & 0x07E0) >> 3;
      gm_blue = ((*(unsigned short *)pt) & 0x001F) << 3;

      *pr = (gm_red * 19595 + gm_green * 38469 + gm_blue * 7472) >> 16;

      pt++;
      pr++;
    }
  }
}

int is_in_array(short x, short y, short height, short width)
{
    if (x >= 0 && x < width && y >= 0 && y < height)
        return 1;
    else
        return 0;
}

void bilinera_interpolation(uint8_t* in_array, short height, short width, 
                            uint8_t* out_array, short out_height, short out_width)
{
//    float h_times = (float)out_height / (float)height,
//           w_times = (float)out_width / (float)width;
    short  x1, y1, x2, y2, f11, f12, f21, f22;
//    float x, y;
		short x, y;
	
    for (int i = 0; i < out_height; i++){
        for (int j = 0; j < out_width; j++){
//            x = j / w_times;
//            y = i / h_times;
	          x = j << 1;
            y = i << 1;
            x1 = (short)(x - 1);
            x2 = (short)(x + 1);
            y1 = (short)(y + 1);
            y2 = (short)(y - 1);
            f11 = is_in_array(x1, y1, height, width) ? in_array[y1*width+x1] : 0;
            f12 = is_in_array(x1, y2, height, width) ? in_array[y2*width+x1] : 0;
            f21 = is_in_array(x2, y1, height, width) ? in_array[y1*width+x2] : 0;
            f22 = is_in_array(x2, y2, height, width) ? in_array[y2*width+x2] : 0;
            out_array[i*out_width+j] = (uint8_t)(((f11 * (x2 - x) * (y2 - y)) +
                                       (f21 * (x - x1) * (y2 - y)) +
                                       (f12 * (x2 - x) * (y - y1)) +
                                       (f22 * (x - x1) * (y - y1))) / ((x2 - x1) * (y2 - y1)));
        }
    }
}

/*!
 * @brief Main function
 */
//int main(void)
int csi_main(void)
{
    BOARD_ConfigMPU();
    BOARD_InitBootPins();
    BOARD_InitDEBUG_UARTPins();
    BOARD_InitSDRAMPins();
    BOARD_EarlyPrepareCamera();
    BOARD_InitCSIPins();
    BOARD_InitLCDPins();
    BOARD_InitBootClocks();
    BOARD_InitDebugConsole();

    PRINTF("CSI RGB565 example start...\r\n");

    memset(s_cameraBuffer, 0, sizeof(s_cameraBuffer));
    memset(s_lcdBuffer, 0, sizeof(s_lcdBuffer));

    APP_InitPxP();

    APP_InitCamera();

    APP_InitDisplay();

    APP_CSI_RGB565();

    while (1)
    {
    }
}

static void APP_InitPxP(void)
{
    /*
     * Configure the PXP for rotate and scale.
     */
    PXP_Init(DEMO_PXP);

    PXP_SetProcessSurfaceBackGroundColor(DEMO_PXP, 0U);

    PXP_SetProcessSurfacePosition(DEMO_PXP, 0U, 0U, DEMO_CAMERA_WIDTH - 1U, DEMO_CAMERA_HEIGHT - 1U);

    /* Disable AS. */
    PXP_SetAlphaSurfacePosition(DEMO_PXP, 0xFFFFU, 0xFFFFU, 0U, 0U);

    PXP_EnableCsc1(DEMO_PXP, false);
}

static void APP_InitCamera(void)
{
    const camera_config_t cameraConfig = {
        .pixelFormat   = kVIDEO_PixelFormatRGB565,
        .bytesPerPixel = APP_CAMERA_BUFFER_BPP,
        .resolution    = FSL_VIDEO_RESOLUTION(DEMO_CAMERA_WIDTH, DEMO_CAMERA_HEIGHT),
        /* Set the camera buffer stride according to panel, so that if
         * camera resoution is smaller than display, it can still be shown
         * correct in the screen.
         */
        .frameBufferLinePitch_Bytes = DEMO_CAMERA_WIDTH * APP_CAMERA_BUFFER_BPP,
        .interface                  = kCAMERA_InterfaceGatedClock,
        .controlFlags               = DEMO_CAMERA_CONTROL_FLAGS,
        .framePerSec                = 30,
    };


    BOARD_InitCameraResource();

    CAMERA_RECEIVER_Init(&cameraReceiver, &cameraConfig, NULL, NULL);

    if (kStatus_Success != CAMERA_DEVICE_Init(&cameraDevice, &cameraConfig))
    {
        PRINTF("Camera device initialization failed\r\n");
        while (1)
        {
            ;
        }
    }

    CAMERA_DEVICE_Start(&cameraDevice);

    /* Submit the empty frame buffers to buffer queue. */
    for (uint32_t i = 0; i < APP_CAMERA_BUFFER_COUNT; i++)
    {
        CAMERA_RECEIVER_SubmitEmptyBuffer(&cameraReceiver, (uint32_t)(s_cameraBuffer[i]));
    }
}

static void APP_InitDisplay(void)
{
    status_t status;

    BOARD_PrepareDisplayController();

    status = g_dc.ops->init(&g_dc);
    if (kStatus_Success != status)
    {
        PRINTF("Display initialization failed\r\n");
        assert(0);
    }

    g_dc.ops->getLayerDefaultConfig(&g_dc, 0, &fbInfo);
    fbInfo.pixelFormat = kVIDEO_PixelFormatXRGB8888;
    fbInfo.width       = DEMO_BUFFER_WIDTH;
    fbInfo.height      = DEMO_BUFFER_HEIGHT;
    fbInfo.startX      = DEMO_BUFFER_START_X;
    fbInfo.startY      = DEMO_BUFFER_START_Y;
    fbInfo.strideBytes = DEMO_BUFFER_WIDTH * APP_FRAME_BUFFER_BPP;
    g_dc.ops->setLayerConfig(&g_dc, 0, &fbInfo);

    g_dc.ops->setCallback(&g_dc, 0, APP_BufferSwitchOffCallback, NULL);

    s_lcdActiveFbIdx = 0;
    s_newFrameShown  = false;
    g_dc.ops->setFrameBuffer(&g_dc, 0, s_lcdBuffer[s_lcdActiveFbIdx]);

    /* For the DBI interface display, application must wait for the first
     * frame buffer sent to the panel.
     */
    if ((g_dc.ops->getProperty(&g_dc) & kDC_FB_ReserveFrameBuffer) == 0)
    {
        while (s_newFrameShown == false)
        {
        }
    }

    s_newFrameShown = true;

    g_dc.ops->enableLayer(&g_dc, 0);
}

#define array_t	void 
static void APP_CSI_RGB565(void)
{
    uint32_t cameraReceivedFrameAddr;
    void *lcdFrameAddr;
		int ret;
		array_t * objects_array;

    pxp_ps_buffer_config_t psBufferConfig = {
        .pixelFormat = kPXP_PsPixelFormatRGB565, /* Note: This is 16-bit per pixel */
        .swapByte    = false,
        .bufferAddrU = 0U,
        .bufferAddrV = 0U,
        .pitchBytes  = DEMO_CAMERA_WIDTH * APP_CAMERA_BUFFER_BPP,
    };

    /* Output config. */
    pxp_output_buffer_config_t outputBufferConfig = {
        .pixelFormat    = kPXP_OutputPixelFormatARGB8888,
        .interlacedMode = kPXP_OutputProgressive,
        .buffer1Addr    = 0U,
        .pitchBytes     = DEMO_BUFFER_WIDTH * APP_FRAME_BUFFER_BPP,
        .width  = DEMO_BUFFER_WIDTH,
        .height = DEMO_BUFFER_HEIGHT,
    };

    CAMERA_RECEIVER_Start(&cameraReceiver);
		
		extern int openmv_testinit();
		openmv_testinit();

    while (1)
    {
        /* Wait to get the full frame buffer to show. */
        while (kStatus_Success != CAMERA_RECEIVER_GetFullBuffer(&cameraReceiver, &cameraReceivedFrameAddr))
        {
        }

        /* Wait for the previous frame buffer is shown, and there is available
            inactive buffer to fill. */
         while (s_newFrameShown == false)
         {
         }
				 
				 /* face detect start */
				 #if 1
				 convert_rgb565_to_gray((uint8_t *)cameraReceivedFrameAddr,(uint8_t *)gray_data,DEMO_CAMERA_WIDTH,DEMO_CAMERA_HEIGHT);
				 extern int openmv_face_detect(unsigned char *pdata,int w,int h,array_t **objects_array);
				 ret = openmv_face_detect((uint8_t *)gray_data,DEMO_CAMERA_WIDTH,DEMO_CAMERA_HEIGHT,&objects_array);
				 				
				 if( ret > 0 )
				{
						draw_rectangle_rgb565((uint16_t *)cameraReceivedFrameAddr,objects_array, DEMO_CAMERA_WIDTH);
//					sprintf(display_str, "#0000ff FACE Detected #"); 
						PRINTF("FACE Detected\r\n");
				}
				else
				{
//					sprintf(display_str, "#00ffff Detecting #"); 
						//PRINTF("Detecting\r\n");
				}
				array_free(objects_array);
				 #else
				 
				 //PRINTF("convert_rgb565_to_gray\r\n");
				 convert_rgb565_to_gray((uint8_t *)cameraReceivedFrameAddr,(uint8_t *)gray_data,DEMO_CAMERA_WIDTH,DEMO_CAMERA_HEIGHT);
				 //zoom_image((uint8_t *)gray_data,(uint8_t *)gray_zoom_data,DEMO_CAMERA_WIDTH>>1,DEMO_CAMERA_HEIGHT>>1);
				 //PRINTF("bilinera_interpolation\r\n");
				 bilinera_interpolation(gray_data, DEMO_CAMERA_HEIGHT, DEMO_CAMERA_WIDTH, 
                           gray_zoom_data, DEMO_CAMERA_HEIGHT>>1, DEMO_CAMERA_WIDTH>>1);
				 //PRINTF("openmv_face_detect\r\n");
				 extern int openmv_face_detect(unsigned char *pdata,int w,int h,array_t **objects_array);
				 ret = openmv_face_detect((uint8_t *)gray_zoom_data,DEMO_CAMERA_WIDTH>>1,DEMO_CAMERA_HEIGHT>>1,&objects_array);

				 if( ret > 0 )
				{
						for (int i = 0; i < array_length(objects_array); i++)
						{
								rectangle_t *r = array_at(objects_array, i);
								r->x <<= 1;
								r->y <<= 1;
								r->w <<= 1;
								r->h <<= 1;
						}
						draw_rectangle_rgb565((uint16_t *)cameraReceivedFrameAddr,objects_array, DEMO_CAMERA_WIDTH);
//					sprintf(display_str, "#0000ff FACE Detected #"); 
						PRINTF("FACE Detected\r\n");
				}
				else
				{
//					sprintf(display_str, "#00ffff Detecting #"); 
						PRINTF("Detecting\r\n");
				}
				array_free(objects_array);
				
				 #endif
				/* face detect end */
				
         /* Convert the camera input picture to RGB format. */
         psBufferConfig.bufferAddr = cameraReceivedFrameAddr;
         PXP_SetProcessSurfaceBufferConfig(DEMO_PXP, &psBufferConfig);

         lcdFrameAddr                   = s_lcdBuffer[s_lcdActiveFbIdx ^ 1];
         outputBufferConfig.buffer0Addr = (uint32_t)lcdFrameAddr;
         PXP_SetOutputBufferConfig(DEMO_PXP, &outputBufferConfig);

         PXP_Start(DEMO_PXP);

         /* Wait for PXP process complete. */
         while (!(kPXP_CompleteFlag & PXP_GetStatusFlags(DEMO_PXP)))
         {
         }
         PXP_ClearStatusFlags(DEMO_PXP, kPXP_CompleteFlag);

         /* Return the camera buffer to camera receiver handle. */
         CAMERA_RECEIVER_SubmitEmptyBuffer(&cameraReceiver, (uint32_t)cameraReceivedFrameAddr);
				 			 
         /* Show the new frame. */
         s_newFrameShown = false;
         g_dc.ops->setFrameBuffer(&g_dc, 0, lcdFrameAddr);
    }
}

static void APP_BufferSwitchOffCallback(void *param, void *switchOffBuffer)
{
    s_newFrameShown = true;
    s_lcdActiveFbIdx ^= 1;
}


#pragma weak application_entry
void application_entry(void *arg)
{
//    while (1) {
//        PRINTF("This is a demo task,please use your task entry!\r\n");
//        tos_task_delay(1000);
//    }
	
	
	  PRINTF("CSI RGB565 example start...\r\n");

    memset(s_cameraBuffer, 0, sizeof(s_cameraBuffer));
    memset(s_lcdBuffer, 0, sizeof(s_lcdBuffer));

    APP_InitPxP();

    APP_InitCamera();

    APP_InitDisplay();

    APP_CSI_RGB565();

    while (1)
    {
			PRINTF("This is a demo task,please use your task entry!\r\n");
			tos_task_delay(1000);
    }
}